//------------------------------------------------------------------------------
//  <copyright file="Pose2DEdge.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics.GraphOptimization
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Numerics.GraphOptimization;
    using ShoNS.Array;

    /// <summary>
    /// Edge for a Pose2D Graph
    /// </summary>
    public class Pose2DEdge : Edge<Pose2D>
    {
        /// <summary>
        /// Initializes a new instance of the Pose2DEdge class
        /// </summary>
        public Pose2DEdge() 
            : base(0, 0, new Pose2D(0, 0, 0), 3)
        {
        }

        /// <summary>
        /// Initializes a new instance of the Pose2DEdge class
        /// </summary>
        /// <param name="measurement">3x1 vector that contains the minimal measurement between nodes $i$ and $j$</param>
        /// <param name="i">$Ith$ node in the graph</param>
        /// <param name="j">$Jth$ node in the graph</param>
        public Pose2DEdge(Pose2D measurement, int i, int j) 
            : base(i, j, measurement, 3)
        {
        }

        /// <summary>
        /// Computes the error assuming the States are Global Poses
        /// </summary>
        /// <param name="pose1">Pose one</param>
        /// <param name="pose2">Pose two</param>
        /// <param name="measurement">Measurement between the two poses</param>
        /// <param name="error">Reference to the double array</param>
        public static void GlobalPoseError(Pose2D pose1, Pose2D pose2, Pose2DEdge measurement, ref double[] error)
        {
            Vector2 tij = new Vector2(measurement.Measurement.X, measurement.Measurement.Y);
           
            double a00 = Math.Cos(measurement.Measurement.Heading);
            double a01 = -Math.Sin(measurement.Measurement.Heading);

            double b00 = Math.Cos(pose1.Heading);
            double b01 = -Math.Sin(pose1.Heading);

            double c00 = Math.Cos(pose2.Heading);
            double c01 = -Math.Sin(pose2.Heading);

            double alpha = a00 * b00 - a01 * b01;
            double beta = -a00 * b01 - a01 * b00;
            
            // error between nodes i and j given a mesurement
            // Rotational error: R_e = R_ij^T * R_i^T * R_j (over parameterized state) (will be identity for 0 error)
            // Rotational error: atan2( R_e(1,0) , R_e(0,0) ) (minimal parameterization)
            // Translation error: R_ij^T * R_i^T * (t_j - t_i) - R_ij^T * t_ij
            error[0] = alpha * (pose2.X - pose1.X) + beta * (pose2.Y - pose1.Y) - (a00 * tij.X - a01 * tij.Y);
            error[1] = -beta * (pose2.X - pose1.X) + alpha * (pose2.Y - pose1.Y) - (a01 * tij.X + a00 * tij.Y);
            error[2] = Math.Atan2(-beta * c00 - alpha * c01, alpha * c00 - beta * c01);
        }

        /// <summary>
        /// Increments a state.  Will be different dependent upon the problem
        /// In the context of PoseSLAM incrementing a state uses the standard motion model
        /// to apply a translation and rotation to a pose.
        /// </summary>
        /// <param name="pose">Pose to increment</param>
        /// <param name="inc">increment to state</param>
        /// <returns>incremented state</returns>
        public static Pose2D GlobalPoseIncrement(Pose2D pose, double[] inc)
        {
            Pose2D increment = new Pose2D(inc[0], inc[1], inc[2]);
            Vector2 t_state = new Vector2(pose.X, pose.Y);
            Vector2 t_inc = new Vector2(increment.X, increment.Y);

            Matrix2 r_state = new Matrix2(Math.Cos(pose.Heading), -Math.Sin(pose.Heading), Math.Sin(pose.Heading), Math.Cos(pose.Heading));
            Matrix2 r_inc = new Matrix2(Math.Cos(increment.Heading), -Math.Sin(increment.Heading), Math.Sin(increment.Heading), Math.Cos(increment.Heading));

            Matrix2 r_new = r_inc * r_state;
            Vector2 t_new = r_inc * t_state + t_inc;

            return new Pose2D(t_new.X, t_new.Y, Math.Atan2(r_new.M10, r_new.M00));
        }

        /// <summary>
        /// Gets the $xth$ basis vector of the parameter space
        /// </summary>
        /// <param name="x">$xth$ basis</param>
        /// <param name="scale">value with which to scale the basis</param>
        /// <returns>scaled $xth$ basis vector</returns>
        public override double[] ScaledBasisVector(int x, double scale)
        {
            double[] basis = new double[3];
            for (int i = 0; i < 3; i++)
            {
                if (x == i)
                {
                    basis[i] = scale;
                }
                else
                {
                    basis[i] = 0;
                }
            }

            return basis;
        }
    }
}
